# one_ugv.launch.py

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    TimerAction,
)
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
)
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    ugv_ids = ['ugv1']
    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])

    return LaunchDescription([


        # 启动 Gazebo 世界（空世界或含地形）

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={'world_sdf': 'ugv_world.sdf','bridge_config':'one_ugv_bridge.yaml'}.items()
        ),

        # Spawn UGV1
        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv1',
                'ugv_pose': '0.0,0.0,1.5,0,0,0',
            }.items()
        ),
        TimerAction(
            period=3.0,  # 延迟时间，单位为秒
            actions=[
                IncludeLaunchDescription(
                    PathJoinSubstitution([launch_dir, 'ugv.launch.py']),
                    launch_arguments={
                        'vehicleID': 'ugv1',
                        'local_host_mode': 'True',
                        'multi_simulation_mode': 'True',
                        'ugv_simulation_mode':'True'
                    }.items()
                )
            ]
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'ugv_allstart.launch.py']),
            launch_arguments={
                'ugv_ids': str(ugv_ids)  # ROS 2 支持 list 作为参数值（YAML 格式）
            }.items()
        ),
        
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'addi_ugv1_bridge.launch.py'])
        )

    ])